#include <behaviortree_cpp/bt_factory.h>
#include <behaviortree_cpp/behavior_tree.h>
#include <filesystem>
#include <fstream>
#include "rclcpp/rclcpp.hpp"
#include "SaySomething.hpp"
#include "SaySomethingWithRos.hpp"

int main()
{
    // 1 必须添加
    rclcpp::init(0, nullptr);  

    // 2 配置行为数
    BT::BehaviorTreeFactory factory;

    // 2.1 注册你的自定义节点
    factory.registerNodeType<SaySomething>("SaySomething");
    factory.registerNodeType<SaySomethingWithRos>("SaySomethingWithRos");

    // 2.2 注入 ROS 2 Node 给 BT 节点用
    // BT::NodeConfiguration config;
    // auto blackboard = BT::Blackboard::create();
    // blackboard->set("node", std::make_shared<rclcpp::Node>());

    // 2.3 加载 XML 行为树
    std::filesystem::path bt_xml = std::filesystem::current_path().append("src/behavior_tree/config/simple_tree_ros.xml");
    auto tree = factory.createTreeFromFile(bt_xml);
    // auto tree = factory.createTreeFromFile(bt_xml, blackboard);

    // 2.4 执行 tick
    auto status = tree.tickOnce();
    while (status == BT::NodeStatus::RUNNING)
    {
        tree.sleep(std::chrono::milliseconds(100));
        status = tree.tickOnce();
    }

    // 3 结束
    rclcpp::shutdown();
    return 0;
}
